#include <iostream>
#include <opencv2/opencv.hpp>

// https://docs.opencv.org/4.5.5/d2/d85/classcv_1_1StereoSGBM.html
// https://www.bilibili.com/read/cv24639156
// An example illustrating the use of the StereoSGBM matching algorithm can be found at opencv_source_code/samples/python/stereo_match.py 
/*
static Ptr<StereoSGBM> cv::StereoSGBM::create ( 	
    int  	minDisparity = 0,
    int  	numDisparities = 16,
    int  	blockSize = 3,
    int  	P1 = 0,
    int  	P2 = 0,
    int  	disp12MaxDiff = 0,
    int  	preFilterCap = 0,
    int  	uniquenessRatio = 0,
    int  	speckleWindowSize = 0,
    int  	speckleRange = 0,
    int  	mode = StereoSGBM::MODE_SGBM 
) 		

minDisparity ：最小视差值，默认为0。

numDisparities ：视差范围，默认为16。必须是16的整数倍。

blockSize ：匹配块大小，默认为3。必须是奇数且大于1。

P1 ：控制视差平滑度的第一个参数，默认为8*blockSize。P1越大，越倾向于生成连续的视差图。

P2 ：控制视差平滑度的第二个参数，默认为32*blockSize。P2越大，越倾向于消除小的视差变化。P2必须大于P1。

disp12MaxDiff ：左右一致性检查时允许的最大视差差异，默认为-1，表示不进行检查。

preFilterCap ：预处理时截断梯度值的上限，默认为63。

uniquenessRatio ：唯一性检查时的阈值，默认为10。表示最佳视差值与次佳视差值之间的比例要大于该阈值才被认为是有效的。

speckleWindowSize ：消除噪声斑点时考虑的窗口大小，默认为0，表示不进行消除。

speckleRange ：消除噪声斑点时考虑的最大视差变化，默认为0，表示不进行消除。

mode ：SGBM算法选择模式，默认为StereoSGBM::MODE_SGBM。可选值有StereoSGBM::MODE_SGBM_3WAY（速度快）、StereoSGBM::MODE_HH4（速度慢）、StereoSGBM::MODE_SGBM（速度中等）、StereoSGBM::MODE_HH（速度慢）。 
*/

using namespace std;
using namespace cv;


int main(int argc, char **argv) {
    /* 读取图片 */
    Mat img_l = imread("../aloeL.jpg");// , COLOR_BGR2GRAY);
    Mat img_r = imread("../aloeR.jpg");// , COLOR_BGR2GRAY);


    int min_disp = 16;
    int num_disp = 96;
    int bsize = 16;
    int window_size = 3;
    int p1 = 8 *3*window_size*window_size;
    int p2 = 32*3*window_size*window_size;
    int disp12MaxDiff = 1;
    int preFilterCap = 0;
    int uniquenessRatio = 10;
    int speckleWindowSize = 100;
    int speckleRange = 32;


    // 设置参数
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(min_disp, num_disp, bsize, p1, p2, disp12MaxDiff, 0, uniquenessRatio, speckleWindowSize, speckleRange);
    

    // 计算视差
    Mat disparity;

for (size_t i = 0; i < 12; i++)
{
    /* code */
    struct timespec time1 = {0};
    clock_gettime(CLOCK_REALTIME, &time1);
    double clc_timestamp = time1.tv_sec + time1.tv_nsec*1e-9;

    sgbm->compute(img_l, img_r, disparity);

    struct timespec time = {0};
    clock_gettime(CLOCK_REALTIME, &time);
    double clc_timestamp2 = time.tv_sec + time.tv_nsec*1e-9;

    cout << "clc_timestamp2 - clc_timestamp" << clc_timestamp2 - clc_timestamp << endl;
}

    



    // 将视差图转换为可视化的图像
    cv::Mat disp_visual;
    disp_visual = (disparity - min_disp) / (num_disp - min_disp);
    disp_visual.convertTo(disp_visual, CV_32F, 1.0 / 16);
    

    imshow("left", img_l);
    imshow("disparity", disp_visual);
    waitKey(0);

    return 0;
}

